#include "motion.h"
#include "algorithm.h"
#include "motor.h"
#include "oled_hardware_i2c.h"
#include "stdio.h"
#include "stdlib.h"
#include "uart.h"
#include <math.h>

#define TAG "Motion"
///////////////////////////Helper Function///////////////////////////
///////////////////////////Helper Function///////////////////////////
void limit_value(int16_t *left_speed, int16_t *right_speed, int16_t max_speed) {
  // 确保最大速度为正值
  int16_t abs_max = abs(max_speed);
  if (abs_max == 0)
    return; // 避免除以零

  // 保存原始速度的符号
  int16_t left_sign = (*left_speed >= 0) ? 1 : -1;
  int16_t right_sign = (*right_speed >= 0) ? 1 : -1;

  // 计算当前速度的绝对值
  int16_t left_abs = abs(*left_speed);
  int16_t right_abs = abs(*right_speed);

  // 找到当前最大速度的绝对值
  int16_t current_max = (left_abs > right_abs) ? left_abs : right_abs;

  // 如果当前速度未超过限制，则不需要调整
  if (current_max <= abs_max) {
    return;
  }

  // 计算缩放比例
  float scale_factor = (float)abs_max / current_max;

  // 按比例缩放速度，使用四舍五入提高精度，保持方向不变
  if (left_abs > 0) {
    *left_speed = (int16_t)round(left_sign * left_abs * scale_factor);
  }
  // 原始速度为0时保持0不变
  else {
    *left_speed = 0;
  }

  if (right_abs > 0) {
    *right_speed = (int16_t)round(right_sign * right_abs * scale_factor);
  }
  // 原始速度为0时保持0不变
  else {
    *right_speed = 0;
  }
}
void motion_torque_ctrl(int16_t left_torque, int16_t right_torque) {
#define SELECT_DIR(torque)                                                     \
  (torque > 0 ? MOTOR_FORWARD : (torque < 0 ? MOTOR_BACKWARD : MOTOR_NEUTRAL))
  motor_set_mode(MOTOR_REAR | MOTOR_LEFT, SELECT_DIR(left_torque));
  motor_set_mode(MOTOR_REAR | MOTOR_RIGHT, SELECT_DIR(right_torque));

  limit_value(&left_torque, &right_torque, 40);

  motor_set_torque(MOTOR_REAR | MOTOR_LEFT, my_abs(left_torque));
  motor_set_torque(MOTOR_REAR | MOTOR_RIGHT, my_abs(right_torque));
}

//////////////////////////End of Helper Function///////////////////////////

///////////////////////Algorithm///////////////////////
///////////////////////Algorithm///////////////////////

//////////////////////MOVE SECTION/////////////////////

static int16_t target_left_speed = 0, target_right_speed = 0, speed = 0;
pid_control_t rl_speed_ctrler = {0}, rr_speed_ctrler = {0}, turn_ctrler = {0};
void motion_speed_ctrl(int16_t left_speed, int16_t right_speed) {
  target_left_speed = left_speed;
  target_right_speed = right_speed;
}

void motion_move_pool(float delata_seconds) {
  float cur_l_speed = motor_get_speed(MOTOR_REAR | MOTOR_LEFT);
  float cur_r_speed = motor_get_speed(MOTOR_REAR | MOTOR_RIGHT);

  int16_t left_error = target_left_speed - (int16_t)cur_l_speed;
  int16_t right_error = target_right_speed - (int16_t)cur_r_speed;

  int16_t adjustment_l =
      pid_update(&rl_speed_ctrler, left_error, delata_seconds);
  int16_t adjustment_r =
      pid_update(&rr_speed_ctrler, right_error, delata_seconds);

  motion_torque_ctrl(target_left_speed + adjustment_l,
                     target_right_speed + adjustment_r);

  // motion_torque_ctrl(target_right_speed,target_left_speed);

  //log(TAG, "left_speed=%f right_speed=%f", cur_l_speed, cur_r_speed);
}

//////////////////////TURN SECTION/////////////////////
void motion_set_speed(int16_t _speed) {
  target_left_speed = _speed;
  target_right_speed = _speed;
  speed = _speed;
}
void motion_turn_pool(float err, float delta_sec, float gain) {
  float adjust = pid_update(&turn_ctrler, err, delta_sec);
  float gain_adjust = speed * gain;

  target_left_speed = speed - (int16_t)round(adjust * gain_adjust);
  target_right_speed = speed + (int16_t)round(adjust * gain_adjust);

  // log(TAG,
  //        "err=%.2f adjust=%.2f gain_adjust=%.2f gain=%.4f
  //        target_left_speed=%d " "target_right_speed=%d base_speed=%d", err,
  //        adjust, gain_adjust,gain, target_left_speed, target_right_speed,
  //        speed)
}

///////////////////////END OF ALGORITHM///////////////////////
///////////////////////END OF ALGORITHM///////////////////////

void motion_pid_rst() {
  pid_init(&rl_speed_ctrler, 2, 0, 0, 50);
  pid_init(&rr_speed_ctrler, 2, 0, 0, 50);
  pid_init(&turn_ctrler, 0.3, 0, 0.01, 50);
}

void motion_init(void) {
  motion_pid_rst();
  motor_init();

  motor_set_mode(MOTOR_REAR | MOTOR_LEFT, MOTOR_NEUTRAL);
  motor_set_mode(MOTOR_REAR | MOTOR_REAR, MOTOR_NEUTRAL);
  motor_set_torque(MOTOR_REAR | MOTOR_LEFT, 0);
  motor_set_torque(MOTOR_REAR | MOTOR_RIGHT, 0);
}